//
// Created by 31853 on 2024/6/14.
//

#include "bsp_uart.h"
#include "stm32h7xx.h"
#include <stdbool.h>
#include <string.h>

#include "fdcan.h"
#include "slowStart.h"
#include "stm32h7xx_hal_uart.h"
#include "Task scheduling_l.h"
#include "UPRE_BSP_WiFi.h"
#include "UPRE_DIJKSTRA.h"
#include "UPRE_Task_Flow.h"
#include "usart.h"

static union Switch_UINT8_T2Float_ {
    uint8_t P[4];
    float value;
} Switch_UINT8_T2Float;

// static uint8_t Rec_Uart7;
static uint8_t Rec_Uart8;

#define RXBUFFERSIZE  256     //最大接收字节数
// uint8_t RxBuffer_Uart7[RXBUFFERSIZE]; //接收数据
// uint8_t Uart7_Rx_Cnt = 0; //接收缓冲计数

uint8_t RxBuffer_Uart8[RXBUFFERSIZE];
uint8_t Uart8_Rx_Cnt = 0;

uint8_t Ball[12] = {0};

void WiFiRecevice_Init(void) {
    HAL_UART_Receive_IT(&huart8, &Rec_Uart8, 1);
}

uint8_t bit_array[32] = {0};

void WiFi_Recevice(void) {
    if (Uart8_Rx_Cnt >= 255) //溢出判断
    {
        // HAL_UART_Transmit(&huart1, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
        // while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
        Uart8_Rx_Cnt = 0;
        memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8));
        // HAL_UART_Transmit(&huart1, (uint8_t *) "data_error\r\n", 12, 0xFFFF);
    } else {
        RxBuffer_Uart8[Uart8_Rx_Cnt++] = Rec_Uart8; //接收数据转存

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 0x4B) && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 0x4F)) {
            // flag_connectFinish = true;
            flag_wifiFinish = true;
            // printf("_2\r\n");
            HAL_UART_Transmit(&huart7, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
            while (HAL_UART_GetState(&huart7) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 'L') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 'I') && (
                RxBuffer_Uart8[Uart8_Rx_Cnt - 3] == 'A') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 4] == 'F')) {
            flag_connectFinish = true;
            // flag_wifiFinish = true;
            // printf("_2\r\n");
            // HAL_UART_Transmit(&huart1, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
            // while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 'R') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 'O') && (
                RxBuffer_Uart8[Uart8_Rx_Cnt - 3] == 'R') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 4] == 'R') && (
                RxBuffer_Uart8[Uart8_Rx_Cnt - 5] == 'E')) {
            flag_linkError = true;

            // HAL_UART_Transmit(&huart1, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
            // while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 0xA3) && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 0xB4)) {
            //            printf("%x\r\n",RxBuffer_Uart8[Uart8_Rx_Cnt - 2]);
            uint8_t data_tx = 0;
            int Index = 0;
            for (int i = 0; i < Uart8_Rx_Cnt; ++i) {
                if (RxBuffer_Uart8[i] == 0x3A && RxBuffer_Uart8[i + 1] == 0x4B) {
                    Index = i + 1;
                    break;
                }
            }
            //            for (int i = 1; i < 17; i++) {
            //                data_tx ^= RxBuffer_Uart8[Index + i];
            //            }
            //            if(RxBuffer_Uart8[Index + 17] == data_tx){

            for (int i = 0; i < 32; ++i) {
                bit_array[i] = ((RxBuffer_Uart8[(i / 8) + Index + 1] >> (i % 8)) & 0x01);
            }

            for (int i = 0; i < 12; ++i) {
                Ball[i] = bit_array[i] & 0x01;
            }

            if (!TaskFlag.Flag_Start) {
                TaskFlag.Flag_Start = bit_array[12] & 0x01;
                if (TaskFlag.Flag_Start) {
                    Main_Control.task_state = game_begin;
                }
            }

            if (!TaskFlag.Flag_ExchangeManualMode) {
                TaskFlag.Flag_ExchangeManualMode = bit_array[13] & 0x01;
            }

            if (!TaskFlag.Flag_Left90) {
                TaskFlag.Flag_Left90 = bit_array[14] & 0x01;
            }

            if (!TaskFlag.Flag_Right90) {
                TaskFlag.Flag_Right90 = bit_array[15] & 0x01;
            }

            if (!TaskFlag.Flag_GetFrontBall) {
                TaskFlag.Flag_GetFrontBall = bit_array[16] & 0x01;
            }

            if (!TaskFlag.Flag_GetRearBall) {
                TaskFlag.Flag_GetRearBall = bit_array[17] & 0x01;
            }

            if (!TaskFlag.Flag_ResetShootBallSpeed) {
                TaskFlag.Flag_ResetShootBallSpeed = bit_array[18] & 0x01;
            }

            if (!TaskFlag.Flag_ShootBall) {
                TaskFlag.Flag_ShootBall = bit_array[19] & 0x01;
            }

            if (!TaskFlag.Flag_third_GetSeding) {
                TaskFlag.Flag_third_GetSeding = bit_array[20] & 0x01;
            }

            if (!TaskFlag.Flag_AddShootBallSpeed) {
                TaskFlag.Flag_AddShootBallSpeed = bit_array[21] & 0x01;
            }

            if (!TaskFlag.Flag_SubtractShootBallSpeed) {
                TaskFlag.Flag_SubtractShootBallSpeed = bit_array[22] & 0x01;
            }
            /////C
            if (!TaskFlag.Flag_StartParking) {
                TaskFlag.Flag_StartParking = bit_array[23] & 0x01;
                if (TaskFlag.Flag_StopParking) {
                    if (TaskFlag.Flag_StartParking) {
                        TaskFlag.Flag_OnceParking = true;
                        TaskFlag.Flag_ManualMode = false;
                        TaskFlag.Flag_StopParking = false;
                        TaskFlag.Flag_StartParking = false;
                    }
                }
            }
            /////D
            if (!TaskFlag.Flag_ExchangePath) {
                TaskFlag.Flag_ExchangePath = bit_array[24] & 0x01;
            }

            if (!TaskFlag.Flag_PathContinue) {
                TaskFlag.Flag_PathContinue = bit_array[25] & 0x01;
            }

            if (!hand_shake.flag_Right) {
                hand_shake.flag_Right = bit_array[26] & 0x01;
            }

            hand_shake.Dir2Zero = bit_array[27] & 0x01;
            hand_shake.Yaw2Zero = bit_array[28] & 0x01;

            if (!TaskFlag.Flag_GetBallColcor) {
                TaskFlag.Flag_GetBallColcor = bit_array[29] & 0x01;
            }
            // memset(&bit_array,0,sizeof bit_array);
            ///TODO:::::用于手动取苗最好固定一侧抓苗，看看是左面抓方便还是右面抓方便，左面是L,右面是R
            ///TODO:::::现在是四个抓手同时打开。
            if (!TaskFlag.Flag_third_PutSeding_L && !TaskFlag.Flag_third_PutSeding_R) {
                TaskFlag.Flag_third_PutSeding_L = bit_array[30] & 0x01;
                TaskFlag.Flag_third_PutSeding_R = bit_array[30] & 0x01;
            }

            Switch_UINT8_T2Float.P[0] = RxBuffer_Uart8[Index + 5];
            Switch_UINT8_T2Float.P[1] = RxBuffer_Uart8[Index + 6];
            Switch_UINT8_T2Float.P[2] = RxBuffer_Uart8[Index + 7];
            Switch_UINT8_T2Float.P[3] = RxBuffer_Uart8[Index + 8];

            hand_shake.HandShake_Vec.dir = Switch_UINT8_T2Float.value;

            Switch_UINT8_T2Float.P[0] = RxBuffer_Uart8[Index + 9];
            Switch_UINT8_T2Float.P[1] = RxBuffer_Uart8[Index + 10];
            Switch_UINT8_T2Float.P[2] = RxBuffer_Uart8[Index + 11];
            Switch_UINT8_T2Float.P[3] = RxBuffer_Uart8[Index + 12];

            hand_shake.HandShake_Vec.size = Switch_UINT8_T2Float.value;

            Switch_UINT8_T2Float.P[0] = RxBuffer_Uart8[Index + 13];
            Switch_UINT8_T2Float.P[1] = RxBuffer_Uart8[Index + 14];
            Switch_UINT8_T2Float.P[2] = RxBuffer_Uart8[Index + 15];
            Switch_UINT8_T2Float.P[3] = RxBuffer_Uart8[Index + 16];
            //                printf("%x\t%x\t%x\t%x\r\n",RxBuffer_Uart8[Index + 13],RxBuffer_Uart8[Index + 14],RxBuffer_Uart8[Index + 15],RxBuffer_Uart8[Index + 16]);
            hand_shake.Handshake_Yaw.yaw = Switch_UINT8_T2Float.value;
            //            }
            // printf("%f\r\n", hand_shake.Handshake_Yaw.yaw);
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }
    }
    WiFiRecevice_Init();
}

void WiFi_Recevice_new(void) {
    if (Uart8_Rx_Cnt >= 255) //溢出判断
    {
        // HAL_UART_Transmit(&huart1, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
        // while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
        Uart8_Rx_Cnt = 0;
        memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8));
        // HAL_UART_Transmit(&huart1, (uint8_t *) "data_error\r\n", 12, 0xFFFF);
    } else {
        RxBuffer_Uart8[Uart8_Rx_Cnt++] = Rec_Uart8; //接收数据转存

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 0x4B) && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 0x4F)) {
            // flag_connectFinish = true;
            flag_wifiFinish = true;
            // printf("_2\r\n");
            HAL_UART_Transmit(&huart7, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
            while (HAL_UART_GetState(&huart7) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 'L') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 'I') && (
                RxBuffer_Uart8[Uart8_Rx_Cnt - 3] == 'A') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 4] == 'F')) {
            flag_connectFinish = true;
            // flag_wifiFinish = true;
            // printf("_2\r\n");
            // HAL_UART_Transmit(&huart1, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
            // while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 'R') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 'O') && (
                RxBuffer_Uart8[Uart8_Rx_Cnt - 3] == 'R') && (RxBuffer_Uart8[Uart8_Rx_Cnt - 4] == 'R') && (
                RxBuffer_Uart8[Uart8_Rx_Cnt - 5] == 'E')) {
            flag_linkError = true;

            // HAL_UART_Transmit(&huart1, (uint8_t *) &RxBuffer_Uart8, Uart8_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
            // while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }

        if ((RxBuffer_Uart8[Uart8_Rx_Cnt - 1] == 0xA3) && (RxBuffer_Uart8[Uart8_Rx_Cnt - 2] == 0xB4)) {
            //            printf("%x\r\n",RxBuffer_Uart8[Uart8_Rx_Cnt - 2]);
            uint8_t data_tx = 0;
            int Index = 0;
            for (int i = 0; i < Uart8_Rx_Cnt; ++i) {
                if (RxBuffer_Uart8[i] == 0x3A && RxBuffer_Uart8[i + 1] == 0x4B) {
                    Index = i + 1;
                    break;
                }
            }
            //            for (int i = 1; i < 17; i++) {
            //                data_tx ^= RxBuffer_Uart8[Index + i];
            //            }
            //            if(RxBuffer_Uart8[Index + 17] == data_tx){

            for (int i = 0; i < 24; ++i) {
                bit_array[i] = ((RxBuffer_Uart8[(i / 8) + Index + 1] >> (i % 8)) & 0x01);
            }

            // for (int i = 0; i < 12; ++i) {
            // Ball[i] = bit_array[i] & 0x01;
            // }

            if (!TaskFlag.Flag_GoTO0) {
                TaskFlag.Flag_GoTO0 = bit_array[0] & 0x01;
            }

            if (!TaskFlag.Flag_GoTO1) {
                TaskFlag.Flag_GoTO1 = bit_array[1] & 0x01;
            }

            if (!TaskFlag.Flag_GoTO2) {
                TaskFlag.Flag_GoTO2 = bit_array[2] & 0x01;
            }

            if (!TaskFlag.Flag_GoTO3) {
                TaskFlag.Flag_GoTO3 = bit_array[3] & 0x01;
            }

            if (!TaskFlag.Flag_GoTO4) {
                TaskFlag.Flag_GoTO4 = bit_array[4] & 0x01;
            }

            if (!TaskFlag.Flag_GoTO5) {
                TaskFlag.Flag_GoTO5 = bit_array[5] & 0x01;
            }

            if (!TaskFlag.Flag_BlueZone) {
                TaskFlag.Flag_BlueZone = bit_array[6] & 0x01;
            }

            if (!TaskFlag.Flag_RedZone) {
                TaskFlag.Flag_RedZone = bit_array[7] & 0x01;
            }

            if(TaskFlag.Flag_BlueZone && TaskFlag.Flag_RedZone) {
                TaskFlag.Flag_BlueZone = false;
                TaskFlag.Flag_RedZone = false;
            } else if(TaskFlag.Flag_BlueZone || TaskFlag.Flag_RedZone) {
                if (!TaskFlag.Flag_Start) {
                    TaskFlag.Flag_Start = bit_array[8] & 0x01;
                    if (TaskFlag.Flag_Start) {
                        Main_Control.task_state = game_begin;
                    }
                }
            }

            if (!TaskFlag.Flag_ExchangeManualMode) {
                TaskFlag.Flag_ExchangeManualMode = bit_array[9] & 0x01;
            }

            if (!TaskFlag.Flag_ReStart) {
                TaskFlag.Flag_ReStart = bit_array[10] & 0x01;
            }

            if (!TaskFlag.Flag_ExchangePath) {
                TaskFlag.Flag_ExchangePath = bit_array[11] & 0x01;
            }

            if (!TaskFlag.Flag_Left90) {
                TaskFlag.Flag_Left90 = bit_array[12] & 0x01;
            }

            if (!TaskFlag.Flag_Right90) {
                TaskFlag.Flag_Right90 = bit_array[13] & 0x01;
            }

            if (!TaskFlag.Flag_GoTwoZone) {
                TaskFlag.Flag_GoTwoZone = bit_array[14] & 0x01;
            }

            if (!TaskFlag.Flag_GetFrontBall) {
                TaskFlag.Flag_GetFrontBall = bit_array[15] & 0x01;
            }

            if (!TaskFlag.Flag_GetRearBall) {
                TaskFlag.Flag_GetRearBall = bit_array[16] & 0x01;
            }

            if (!TaskFlag.Flag_ResetShootBallSpeed) {
                TaskFlag.Flag_ResetShootBallSpeed = bit_array[17] & 0x01;
            }

            if (!TaskFlag.Flag_ShootBall) {
                TaskFlag.Flag_ShootBall = bit_array[18] & 0x01;
            }

            if (!TaskFlag.Flag_ManualGetSeding) {
                TaskFlag.Flag_ManualGetSeding = bit_array[19] & 0x01;
            }

            if (!TaskFlag.Flag_AddShootBallSpeed)  {
                TaskFlag.Flag_AddShootBallSpeed = bit_array[20] & 0x01;
            }

            if(!TaskFlag.Flag_SubtractShootBallSpeed) {
                TaskFlag.Flag_SubtractShootBallSpeed = bit_array[21]  & 0x01;
            }

            // if(!TaskFlag.Flag_ShootBall) {
                // TaskFlag.Flag_ShootBall = bit_array[22] & 0x01;
            // }

            if(!hand_shake.flag_Left) {
                hand_shake.flag_Left = bit_array[22] & 0x01;
            }

            if (!hand_shake.flag_Right) {
                hand_shake.flag_Right = bit_array[23] & 0x01;
            }

            Switch_UINT8_T2Float.P[0] = RxBuffer_Uart8[Index + 4];
            Switch_UINT8_T2Float.P[1] = RxBuffer_Uart8[Index + 5];
            Switch_UINT8_T2Float.P[2] = RxBuffer_Uart8[Index + 6];
            Switch_UINT8_T2Float.P[3] = RxBuffer_Uart8[Index + 7];

            hand_shake.HandShake_Vec.dir = Switch_UINT8_T2Float.value;

            Switch_UINT8_T2Float.P[0] = RxBuffer_Uart8[Index + 8];
            Switch_UINT8_T2Float.P[1] = RxBuffer_Uart8[Index + 9];
            Switch_UINT8_T2Float.P[2] = RxBuffer_Uart8[Index + 10];
            Switch_UINT8_T2Float.P[3] = RxBuffer_Uart8[Index + 11];

            hand_shake.HandShake_Vec.size = Switch_UINT8_T2Float.value;

            Switch_UINT8_T2Float.P[0] = RxBuffer_Uart8[Index + 12];
            Switch_UINT8_T2Float.P[1] = RxBuffer_Uart8[Index + 13];
            Switch_UINT8_T2Float.P[2] = RxBuffer_Uart8[Index + 14];
            Switch_UINT8_T2Float.P[3] = RxBuffer_Uart8[Index + 15];
            //                printf("%x\t%x\t%x\t%x\r\n",RxBuffer_Uart8[Index + 13],RxBuffer_Uart8[Index + 14],RxBuffer_Uart8[Index + 15],RxBuffer_Uart8[Index + 16]);
            hand_shake.Handshake_Yaw.yaw = Switch_UINT8_T2Float.value;
            //            }
            // printf("%f\r\n", hand_shake.Handshake_Yaw.yaw);
            Uart8_Rx_Cnt = 0;
            memset(RxBuffer_Uart8, 0x00, sizeof(RxBuffer_Uart8)); //清空数组; //将收到的信息发送出去
        }
    }
    WiFiRecevice_Init();
}
